#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <mpi.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_heapsort.h>

#include "halofinder.h"

void test_fofdata(FoFData *fofd)
{
  int numHead,numTail;
  int i,j,k,found_neighbor,len;
  float L,halfL,rad,exclrad,dx,dy,dz;
  
  L = (float) (haloFinderData.BoxLengthSnapshot);
  halfL = (float) (haloFinderData.BoxLengthSnapshot/2.0);
  
  //test the groups
  // 1) make sure the number of head == i and tail == -1 are the same 
  // 2) make sure tail link is -1
  // 3) make sure length is correct
  // 4) check to make sure all group members are group members
  // 2) check to make sure all links are group members
  numHead = 0;
  numTail = 0;
  for(i=0;i<NumHalos;++i)
    {
      assert(fofd[fofd[fofd[i].head].tail].link == -1);
      
      if(fofd[i].link == -1)
	++numTail;
      
      if(fofd[i].head == i)
	{
	  
	  ++numHead;
      
	  len = 0;
	  k = fofd[i].head;
	  while(k >= 0)
	    {
	      ++len;
	      
	      found_neighbor = 0;
	      j = fofd[i].head;                                                                                                                                                                                                                                             
	      while(j >= 0)
		{
		  dx = (float) (fabs(Halos[k].pos[0] - Halos[j].pos[0]));
		  if(dx > halfL)
		    dx = L - dx;
		  dy = (float) (Halos[k].pos[1] - Halos[j].pos[1]);
		  if(dy > halfL)
		    dy = L - dy;
		  dz = (float) (Halos[k].pos[2] - Halos[j].pos[2]);
		  if(dz > halfL)
		    dz = L - dz;
		  
		  rad = (float) (sqrt(dx*dx + dy*dy + dz*dz));
		  exclrad = halo_exlcusion_radius(Halos[k].mass*haloFinderData.PartMass,Halos[j].mass*haloFinderData.PartMass);
		  if(rad <  exclrad)
		    {
		      found_neighbor = 1;
		      break;
		    }
		  
		  j = fofd[j].link;
		}
	      
	      assert(found_neighbor);                                 
	      
	      for(j=0;j<NumHalos;++j)
		{
		  dx = (float) (fabs(Halos[k].pos[0] - Halos[j].pos[0]));
		  if(dx > halfL)
		    dx = L - dx;
		  dy = (float) (Halos[k].pos[1] - Halos[j].pos[1]);
		  if(dy > halfL)
		    dy = L - dy;
		  dz = (float) (Halos[k].pos[2] - Halos[j].pos[2]);
		  if(dz > halfL)
		    dz = L - dz;
		  
		  rad = (float) (sqrt(dx*dx + dy*dy + dz*dz));
		  exclrad = halo_exlcusion_radius(Halos[k].mass*haloFinderData.PartMass,Halos[j].mass*haloFinderData.PartMass);
		  if(rad < exclrad)
		    {
		      if(fofd[j].head != fofd[k].head)
			fprintf(stderr,"%d: rad = %f, exclrad = %f, head,length[%d] = %d|%d, head,length[%d] = %d|%d\n",ThisTask,
				rad,exclrad,j,fofd[j].head,fofd[fofd[j].head].length,k,fofd[k].head,fofd[fofd[k].head].length);
		      assert(fofd[j].head == fofd[k].head);
		    }
		}
	      
	      k = fofd[k].link;
	    }
	  
	  if(len != fofd[i].length)
	    fprintf(stderr,"%d: len = %d, grouplen = %d\n",ThisTask,len,fofd[i].length);
	  assert(len = fofd[i].length);
	}
    }
  fprintf(stderr,"%d: numHead = %d, numTail = %d\n",ThisTask,numHead,numTail);
  assert(numHead == numTail);
  
}

void test_fofhalogroupfinder(void)
{
  int i,periodic;
  gsl_rng *rng;
  float domainLengths[3];
  float *px,*py,*pz;
  kdTreeData *td;
  FoFData *fofd;
  float lambda = (float) (1.0/1000.0);
  FILE *fp;
  char name[1024];
  int maxNumNbrs,*nbrsInd;
  float *nbrsRad2;
  float maxMass,globalMaxMass;
  float searchRadius;
  long TotNumHalos;
  int j,k,found_neighbor;
  float L,halfL,rad,exclrad,dx,dy,dz;

  L = (float) (haloFinderData.BoxLengthSnapshot);
  halfL = (float) (haloFinderData.BoxLengthSnapshot/2.0);
  
  rng = gsl_rng_alloc(gsl_rng_ranlxd2);
  gsl_rng_set(rng,(unsigned long) (ThisTask+1));

  setupdomaindecomp();
  
  domainLengths[0] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[0])) + 2.0*haloFinderData.DomainBuffSize);
  domainLengths[1] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[1])) + 2.0*haloFinderData.DomainBuffSize);
  domainLengths[2] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[2])) + 2.0*haloFinderData.DomainBuffSize);
  
  periodic = 0;
  
  NumHalos = 1000;
  Halos = (Halo*)malloc(sizeof(Halo)*NumHalos);
  assert(Halos != NULL);
  for(i=0;i<NumHalos;++i)
    {
      Halos[i].mass = (float) (-1.0*log(1.0 - gsl_rng_uniform(rng))/lambda);
      Halos[i].pos[0] = (float) (gsl_rng_uniform(rng)*domainLengths[0]);
      Halos[i].pos[1] = (float) (gsl_rng_uniform(rng)*domainLengths[1]);
      Halos[i].pos[2] = (float) (gsl_rng_uniform(rng)*domainLengths[2]);
      Halos[i].vcirc = Halos[i].mass;
      //if(ThisTask == 0)
      //fprintf(stderr,"%d: halo ind = %ld, pos = %f|%f|%f, mass = %lf\n",ThisTask,i,Halos[i].pos[0],Halos[i].pos[1],Halos[i].pos[2],Halos[i].mass);
    }
  
  gatherpeakshalos();
  scatterpeakshalos();
  
  fprintf(stderr,"%d: NumHalos = %ld\n",ThisTask,NumHalos);
  
  MPI_Allreduce(&NumHalos,&TotNumHalos,1,MPI_LONG,MPI_SUM,MPI_COMM_WORLD);
  if(ThisTask == 0)
    fprintf(stderr,"begin TotNumHalos = %ld\n",TotNumHalos);
  
  //setup for searching
  maxNumNbrs = 100000;
  nbrsRad2 = (float*)malloc(sizeof(float)*maxNumNbrs);
  assert(nbrsRad2 != NULL);
  nbrsInd = (int*)malloc(sizeof(int)*maxNumNbrs);
  assert(nbrsInd != NULL);
  
  //get search radius
  maxMass = Halos[0].mass;
  for(i=0;i<NumHalos;++i)
    if(Halos[i].mass > maxMass)
      maxMass = Halos[i].mass;
  MPI_Allreduce(&maxMass,&globalMaxMass,1,MPI_FLOAT,MPI_MAX,MPI_COMM_WORLD);
  searchRadius = (float) (halo_exlcusion_radius(globalMaxMass*haloFinderData.PartMass,globalMaxMass*haloFinderData.PartMass)*2.00);
  
  //build kdtree                                                                                                                                                          
  px = (float*)malloc(sizeof(float)*NumHalos);
  assert(px != NULL);
  py = (float*)malloc(sizeof(float)*NumHalos);
  assert(py != NULL);
  pz = (float*)malloc(sizeof(float)*NumHalos);
  assert(pz != NULL);
  for(i=0;i<NumHalos;++i)
    {
      px[i] = Halos[i].pos[0];
      py[i] = Halos[i].pos[1];
      pz[i] = Halos[i].pos[2];
    }
  td = buildkdTree(px,py,pz,(int) NumHalos,NULL);

  //build fof groups of halos
  fofd = build_fofhalogroups(td,px,py,pz,periodic,searchRadius,&nbrsRad2,&nbrsInd,&maxNumNbrs);
  
  fprintf(stderr,"%d: found groups\n",ThisTask);
  
  MPI_Allreduce(&NumHalos,&TotNumHalos,1,MPI_LONG,MPI_SUM,MPI_COMM_WORLD);
  if(ThisTask == 0)
    fprintf(stderr,"build TotNumHalos = %ld\n",TotNumHalos);
  
  test_fofdata(fofd);
  
  fprintf(stderr,"%d: all groups passed tests for build\n",ThisTask);
  
  join_fofhalogroups(&td,&px,&py,&pz,periodic,searchRadius,&nbrsRad2,&nbrsInd,&maxNumNbrs,&fofd);
  
  fprintf(stderr,"%d: # of halos after join = %ld\n",ThisTask,NumHalos);
  
  MPI_Allreduce(&NumHalos,&TotNumHalos,1,MPI_LONG,MPI_SUM,MPI_COMM_WORLD);
  if(ThisTask == 0)
    fprintf(stderr,"join TotNumHalos = %ld\n",TotNumHalos);
  
  test_fofdata(fofd);
  
  fprintf(stderr,"%d: all groups passed tests for join, # of halos = %ld\n",ThisTask,NumHalos);
  
  percolate_fofhalogroups(periodic,fofd);
  
  fprintf(stderr,"%d: done with peroclation, # of halos = %ld\n",ThisTask,NumHalos);
  
  shifthalosdomain2global();
  
  gatherpeakshalos();
  
  //test peroclation
  if(ThisTask == 0)
    {
      fprintf(stderr,"%d: %ld halos in final sample\n",ThisTask,NumHalos);
      
      for(j=0;j<NumHalos;++j)
	{
	  found_neighbor = 0;
	  for(k=0;k<NumHalos;++k)
	    {
	      dx = (float) (fabs(Halos[k].pos[0] - Halos[j].pos[0]));
	      dy = (float) (Halos[k].pos[1] - Halos[j].pos[1]);
	      dz = (float) (Halos[k].pos[2] - Halos[j].pos[2]);
	      
	      if(periodic)
		{
		  if(dx > halfL)
		    dx = L - dx;
		  if(dy > halfL)
		    dy = L - dy;
		  if(dz > halfL)
		    dz = L - dz;
		}
	      
	      rad = (float) (sqrt(dx*dx + dy*dy + dz*dz));
	      exclrad = halo_exlcusion_radius(Halos[k].mass*haloFinderData.PartMass,Halos[j].mass*haloFinderData.PartMass);
	      if(rad <  exclrad && j != k)
		{
		  fprintf(stderr,"%d: ind,pos = %ld|%f|%f|%f, ind,pos = %ld|%f|%f|%f, rad = %f, exclrad = %f\n",ThisTask,
			  Halos[j].index,Halos[j].pos[0],Halos[j].pos[1],Halos[j].pos[2],
			  Halos[k].index,Halos[k].pos[0],Halos[k].pos[1],Halos[k].pos[2],
			  rad,exclrad);
		  found_neighbor = 1;
		  break;
		}
	    }
	  
	  assert(found_neighbor == 0);
	}
      
      fprintf(stderr,"%d: all halos passed percolation\n",ThisTask);
    }
  
  /*if(0)
    {
      sprintf(name,"fofhalogroups.%d",ThisTask);
      fp = fopen(name,"w");
      assert(fp != NULL);
      for(i=0;i<fofd->NumGroups;++i)
	{
	  k = fofd->groups[i].head;
	  while(k >= 0)
	    {
	      fprintf(fp,"%.10e\t%.10e\t%.10e\t%d\n",Halos[k].pos[0],Halos[k].pos[1],Halos[k].pos[2],fofd->groupnum[k]);
	      k = fofd->links[k];
	    }
	}
      fclose(fp);
    }
  */
  
  //free stuff  
  free(nbrsRad2);
  free(nbrsInd);
  free(fofd);
  destroyhalos();
  destroydomaindecomp();
  gsl_rng_free(rng);
  destroykdTree(td);
  free(px);
  free(py);
  free(pz);
}

void test_sort_halos(void)
{
  long i;
  gsl_rng *rng;
  double time=0.0;
  
  NumHalos = 100000000;
  Halos = (Halo*)malloc(sizeof(Halo)*NumHalos);
  assert(Halos != NULL);
  
  rng = gsl_rng_alloc(gsl_rng_ranlxd2);
  gsl_rng_set(rng,(unsigned long) (ThisTask+1));
  
  for(i=0;i<NumHalos;++i)
    {
      Halos[i].mass = (float) (gsl_rng_uniform(rng)*10000.0);
      Halos[i].vcirc = (float) (gsl_rng_uniform(rng)*10000.0);
    }
  
  //test for mass
  time -= MPI_Wtime();
  sort_halos_mass();
  time += MPI_Wtime();
  for(i=1;i<NumHalos;++i)
    assert(Halos[i-1].mass >= Halos[i].mass);
  fprintf(stderr,"%d: halos sorted by mass passed!\n",ThisTask);
  
  time -= MPI_Wtime();
  sort_halos_vcirc();
  time += MPI_Wtime();
  for(i=1;i<NumHalos;++i)
    assert(Halos[i-1].vcirc >= Halos[i].vcirc);
  fprintf(stderr,"%d: halos sorted by vcirc passed!\n",ThisTask);
  
  fprintf(stderr,"%d: time for sort of %ld halos is %f [s]\n",ThisTask,NumHalos,time/2.0);
  
  free(Halos);
  NumHalos = 0;
  Halos = NULL;
  gsl_rng_free(rng);
}

typedef struct {
  int index;
  float rad2;
} KNNbrsSort;

static int compKNNbrsIndexSortRad(const void *a, const void *b)
{
  if(((const KNNbrsSort*)a)->rad2 > ((const KNNbrsSort*)b)->rad2)
    return 1;
  else if(((const KNNbrsSort*)a)->rad2 < ((const KNNbrsSort*)b)->rad2)
    return -1;
  else
    return 0;
}

typedef union {
  float d;
  peanokey key;
} floatKeyUnion;

typedef struct {
  int index;
  floatKeyUnion u;
} SortPart;

static int compPeanoKey(const void *a, const void *b)
{
  if(((const SortPart*)a)->u.key > ((const SortPart*)b)->u.key)
    return 1;
  else if(((const SortPart*)a)->u.key < ((const SortPart*)b)->u.key)
    return -1;
  else
    return 0;
}

static void reorder_parts_phcurve(long Np, float *px, float *py, float *pz, float domainLengths[3])
{
  int i,j;
  float *pos[3];
  SortPart *sp;
  double sortTime = 0.0;
  int N,x,y,z;
  double dx,dy,dz;
  
#ifdef DEBUG
  if(ThisTask == 0)
    fprintf(stderr,"%d: redordering parts according to peano-hilbert index\n",ThisTask);
#endif

  //setup
  pos[0] = px;
  pos[1] = py;
  pos[2] = pz;
  
  N = 1;
  N = N << BITS_PER_DIMENSION;
  
  dx = domainLengths[0]/((double) (N));
  dy = domainLengths[1]/((double) (N));
  dz = domainLengths[2]/((double) (N));
  
#ifdef DEBUG
  if(ThisTask == 0)
    fprintf(stderr,"%d: starting sort of parts\n",ThisTask);
#endif
  sp = (SortPart*)malloc(sizeof(SortPart)*Np);
  assert(sp != NULL);
  for(i=0;i<Np;++i)
    {
      sp[i].index = i;
      x = (int) (px[i]/dx);
      if(x == N)
        --x;
      y = (int) (py[i]/dy);
      if(y == N)
        --y;
      z = (int) (pz[i]/dz);
      if(z == N)
        --z;
      sp[i].u.key = peano_hilbert_key(x,y,z,BITS_PER_DIMENSION);
    }
  sortTime -= MPI_Wtime();
  qsort(sp,(size_t) Np,sizeof(SortPart),compPeanoKey);
  sortTime += MPI_Wtime();
#ifdef DEBUG
  if(ThisTask == 0)  
    fprintf(stderr,"%d: done with sort of parts, time = %f [s]\n",ThisTask,sortTime);
#endif

  for(i=0;i<3;++i)
    {
#ifdef DEBUG
      if(ThisTask == 0)
        fprintf(stderr,"%d: on pos = %d\n",ThisTask,i);
#endif
      
      for(j=0;j<Np;++j)
        sp[j].u.d = pos[i][sp[j].index];
      
      for(j=0;j<Np;++j)
        pos[i][j] = sp[j].u.d;
    }
  
  free(sp);
    
#ifdef DEBUG
  if(ThisTask == 0)
    fprintf(stderr,"%d: done redordering parts according to peano-hilbert index\n",ThisTask);
#endif
}

void test_kdtree_knnbrsfind(void)
{
  kdTreeData *td;
  float domainLengths[3],centPos[3];
  float dx,dy,dz;
  int NumKeepParts,NumNbrs,retNumNbrs,periodic,i;
  gsl_rng *rng;
  float *nbrsRad2;
  int *nbrsInd;
  float *px,*py,*pz;
  KNNbrsSort *ns,*nsf;
  float L,halfL;
  double time = 0.0;
  long Ntest=100;
  int k;
  float dlims[3][2];

  L = haloFinderData.BoxLengthSnapshot;
  halfL = (float) (L/2.0);
  
  rng = gsl_rng_alloc(gsl_rng_ranlxd2);
  gsl_rng_set(rng,(unsigned long) (ThisTask+1));
  
  NumKeepParts = 10000000;
  domainLengths[0] = L;
  domainLengths[1] = L;
  domainLengths[2] = L;
  NumNbrs = 65;
  for(i=0;i<3;++i)
    {
      dlims[i][0] = 0.0;
      dlims[i][1] = L;
    }
  
  //set max num nbrs
  nbrsRad2 = (float*)malloc(sizeof(float)*NumNbrs);
  assert(nbrsRad2 != NULL);
  nbrsInd = (int*)malloc(sizeof(int)*NumNbrs);
  assert(nbrsInd != NULL);
  nsf = (KNNbrsSort*)malloc(sizeof(KNNbrsSort)*NumNbrs);
  assert(nsf != NULL);
  
  //make parts
  ns = (KNNbrsSort*)malloc(sizeof(KNNbrsSort)*NumKeepParts);
  assert(ns != NULL);
  px = (float*)malloc(sizeof(float)*NumKeepParts);
  assert(px != NULL);
  py = (float*)malloc(sizeof(float)*NumKeepParts);
  assert(py != NULL);
  pz = (float*)malloc(sizeof(float)*NumKeepParts);
  assert(pz != NULL);
  for(i=0;i<NumKeepParts;++i)
    {
      px[i] = (float) (gsl_rng_uniform(rng)*domainLengths[0]);
      py[i] = (float) (gsl_rng_uniform(rng)*domainLengths[1]);
      pz[i] = (float) (gsl_rng_uniform(rng)*domainLengths[2]);
    }
  reorder_parts_phcurve(NumKeepParts,px,py,pz,domainLengths);
  
  for(periodic=0;periodic<2;++periodic)
    {
      td = buildkdTree(px,py,pz,(int) NumKeepParts,dlims);
      fprintf(stderr,"%d: domain = %f|%f|%f to %f|%f|%f\n",ThisTask,td->treeNodes[0].baseLoc[0],td->treeNodes[0].baseLoc[1],td->treeNodes[0].baseLoc[2],
	      td->treeNodes[0].baseLoc[0]+td->treeNodes[0].sideLengths[0],
	      td->treeNodes[0].baseLoc[1]+td->treeNodes[0].sideLengths[1],
	      td->treeNodes[0].baseLoc[2]+td->treeNodes[0].sideLengths[2]);
      
      for(k=0;k<=4;++k)
	{
	  if(periodic && (k < 0 || k > 4))
	    continue;
	  
	  centPos[0] = (float) (k*halfL/2.0);
	  centPos[1] = (float) (k*halfL/2.0);
	  centPos[2] = (float) (k*halfL/2.0);
	  	  
	  for(i=0;i<NumKeepParts;++i)
	    {
	      dx = (float) (fabs(px[i] - centPos[0]));
	      dy = (float) (fabs(py[i] - centPos[1]));
	      dz = (float) (fabs(pz[i] - centPos[2]));
	      if(periodic)
		{
		  if(dx > halfL)
		    dx = L - dx;
		  
		  if(dy > halfL)
		    dy = L - dy;
		  
		  if(dz > halfL)
		    dz = L - dz;
		}
	      
	      ns[i].rad2 = (float) ((dx*dx + dy*dy + dz*dz));
	      ns[i].index = i;
	    }
	  
	  qsort(ns,(size_t) NumKeepParts,sizeof(KNNbrsSort),compKNNbrsIndexSortRad);
	  
	  time -= MPI_Wtime();
	  for(i=0;i<Ntest;++i)
	    retNumNbrs = get_knnbrs_kdtree(centPos,periodic,haloFinderData.BoxLengthSnapshot,nbrsRad2,nbrsInd,NumNbrs,px,py,pz,td);
	  time += MPI_Wtime();
	  
	  if(NumNbrs > NumKeepParts)
	    {
	      fprintf(stderr,"%d: retNumNbrs = %d (?= %d), time = %lf [s]\n",ThisTask,retNumNbrs,NumKeepParts,time/100.0);
	      assert(retNumNbrs == NumKeepParts);
	    }
	  else
	    {
	      fprintf(stderr,"%d: retNumNbrs = %d (?= %d), time = %lf [s]\n",ThisTask,retNumNbrs,NumNbrs,time/100.0);
	      assert(retNumNbrs == NumNbrs);
	    }
	  
	  for(i=0;i<retNumNbrs;++i)
	    {
	      nsf[i].rad2 = nbrsRad2[i];
	      nsf[i].index = nbrsInd[i];
	    }
	  qsort(nsf,(size_t) retNumNbrs,sizeof(KNNbrsSort),compKNNbrsIndexSortRad);
	  
	  for(i=0;i<retNumNbrs;++i)
	    {
	      if(ns[i].index != nsf[i].index || ns[i].rad2 != nsf[i].rad2)
		fprintf(stderr,"%d: ns,nsf index = %d|%d, ns,nsf rad2 = %f|%f\n",ThisTask,ns[i].index,nsf[i].index,ns[i].rad2,nsf[i].rad2);
	      assert(ns[i].index == nsf[i].index);
	      assert(ns[i].rad2 == nsf[i].rad2);
	    }
	  
	  fprintf(stderr,"%d: all knnbrs passed test!\n",ThisTask);
	}
      
      destroykdTree(td);
    }
  
  free(nbrsRad2);
  free(nbrsInd);
  free(px);
  free(py);
  free(pz);
  free(ns);
  free(nsf);
  gsl_rng_free(rng);
}

static int compHalosMass(const void *a, const void *b)
{
  if(((const Halo*)a)->mass > ((const Halo*)b)->mass)
    return -1;
  else if(((const Halo*)a)->mass < ((const Halo*)b)->mass)
    return 1;
  else
    return 0;
}

void test_peakhaloio(void)
{
  long i,baseNumHalosPerTask = 10000;
  Halo *baseHalos;
  float domainOrigin[3],domainLengths[3];
  gsl_rng *rng;
  
  rng = gsl_rng_alloc(gsl_rng_ranlxd2);
  gsl_rng_set(rng,(unsigned long) (ThisTask+1));
  
  setupdomaindecomp();
  
  domainOrigin[0] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[0]))*((float) (domainCoords[0])));
  domainOrigin[1] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[1]))*((float) (domainCoords[1])));
  domainOrigin[2] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[2]))*((float) (domainCoords[2])));
  
  domainLengths[0] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[0])));
  domainLengths[1] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[1])));
  domainLengths[2] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[2])));

  baseHalos = (Halo*)malloc(sizeof(Halo)*baseNumHalosPerTask);
  assert(baseHalos != NULL);
  
  NumHalos = baseNumHalosPerTask;
  Halos = (Halo*)malloc(sizeof(Halo)*NumHalos);
  assert(Halos != NULL);
  
  for(i=0;i<baseNumHalosPerTask;++i)
    {
      baseHalos[i].mass = (float) (ThisTask*baseNumHalosPerTask + i);
      baseHalos[i].pos[0] = (float) ((gsl_rng_uniform(rng)/1.0 + 0.0)*domainLengths[0] + domainOrigin[0]);
      baseHalos[i].pos[1] = (float) ((gsl_rng_uniform(rng)/1.0 + 0.0)*domainLengths[1] + domainOrigin[1]);
      baseHalos[i].pos[2] = (float) ((gsl_rng_uniform(rng)/1.0 + 0.0)*domainLengths[2] + domainOrigin[2]);
      Halos[i] = baseHalos[i];
    }
  
  writepeaks();
  destroyhalos();
  readpeaks();
  
  fprintf(stderr,"%d: NumHalos = %ld\n",ThisTask,NumHalos);
  
  assert(NumHalos == baseNumHalosPerTask);
  gsl_heapsort(Halos,(size_t) NumHalos,sizeof(Halo),compHalosMass);
  for(i=0;i<baseNumHalosPerTask;++i)
    {
      //fprintf(stderr,"%d: base mass = %f, mass = %f\n",ThisTask,baseHalos[i].mass,Halos[baseNumHalosPerTask - i - 1].mass);
      assert(baseHalos[i].mass == Halos[baseNumHalosPerTask - i - 1].mass);
    }
  
  fprintf(stderr,"%d: read/write peaks passed!\n",ThisTask);
  
  writehalos();
  destroyhalos();
  readhalos();
  
  assert(NumHalos == baseNumHalosPerTask);
  gsl_heapsort(Halos,(size_t) NumHalos,sizeof(Halo),compHalosMass);
  for(i=0;i<baseNumHalosPerTask;++i)
    {
      //fprintf(stderr,"%d: base mass = %f, mass = %f\n",ThisTask,baseHalos[i].mass,Halos[baseNumHalosPerTask - i - 1].mass);
      assert(baseHalos[i].mass == Halos[baseNumHalosPerTask - i - 1].mass);
    }
  
  fprintf(stderr,"%d: read/write halos passed!\n",ThisTask);
  
  destroyhalos();
  free(baseHalos);
  gsl_rng_free(rng);

  destroydomaindecomp();
}

void test_exchangebufferregions(void)
{
  long i,TotNumParts,TotNumPartsInDomain;
  FILE *fp;
  char name[MAX_FILENAME];
  
  setupdomaindecomp();
  
  readpartsfromsnapshot();
  
  MPI_Reduce(&NumParts,&TotNumParts,1,MPI_LONG,MPI_SUM,0,MPI_COMM_WORLD);
  if(ThisTask == 0)
    fprintf(stderr,"TotNumParts = %ld\n",TotNumParts);
  
  exchangebufferregions();
  
  MPI_Reduce(&NumPartsInDomain,&TotNumPartsInDomain,1,MPI_LONG,MPI_SUM,0,MPI_COMM_WORLD);
  if(ThisTask == 0)
    fprintf(stderr,"TotNumPartsInDomain = %ld\n",TotNumPartsInDomain);
  MPI_Reduce(&NumParts,&TotNumParts,1,MPI_LONG,MPI_SUM,0,MPI_COMM_WORLD);
  if(ThisTask == 0)
    fprintf(stderr,"TotNumParts = %ld\n",TotNumParts);
  
  //shiftpartshalosglobal2domain();
  
  sprintf(name,"%s.actuallyparts.test.%d",haloFinderData.HaloOutputFile,ThisTask);
  fp = fopen(name,"w");
  for(i=0;i<NumParts;++i)
    if(i < NumPartsInDomain)
      fprintf(fp,"%f\t%f\t%f\t%d\n",PartsPx[i],PartsPy[i],PartsPz[i],1);
    else
      fprintf(fp,"%f\t%f\t%f\t%d\n",PartsPx[i],PartsPy[i],PartsPz[i],0);
  fclose(fp);
  
  destroyparts();
  destroydomaindecomp();
}

void test_shiftpartshalos(void)
{
  long i;
  gsl_rng *rng;
  float domainOrigin[3],domainLengths[3];
  
  rng = gsl_rng_alloc(gsl_rng_ranlxd2);
  gsl_rng_set(rng,(unsigned long) (ThisTask+1));

  setupdomaindecomp();
  
  domainOrigin[0] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[0]))*((float) (domainCoords[0])) - haloFinderData.DomainBuffSize);
  domainOrigin[1] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[1]))*((float) (domainCoords[1])) - haloFinderData.DomainBuffSize);
  domainOrigin[2] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[2]))*((float) (domainCoords[2])) - haloFinderData.DomainBuffSize);
  
  domainLengths[0] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[0])) + 2.0*haloFinderData.DomainBuffSize);
  domainLengths[1] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[1])) + 2.0*haloFinderData.DomainBuffSize);
  domainLengths[2] = (float) (haloFinderData.BoxLengthSnapshot/((float) (NumDomains[2])) + 2.0*haloFinderData.DomainBuffSize);
  
  if(ThisTask == 1)
    {
      NumHalos = 10;
      Halos = (Halo*)malloc(sizeof(Halo)*NumHalos);
      assert(Halos != NULL);
      for(i=0;i<NumHalos;++i)
	{
	  Halos[i].pos[0] = (float) (gsl_rng_uniform(rng)*domainLengths[0] + domainOrigin[0]);
	  Halos[i].pos[1] = (float) (gsl_rng_uniform(rng)*domainLengths[1] + domainOrigin[1]);
	  Halos[i].pos[2] = (float) (gsl_rng_uniform(rng)*domainLengths[2] + domainOrigin[2]);
	  fprintf(stderr,"%d: halo ind = %ld, pos = %f|%f|%f\n",ThisTask,i,Halos[i].pos[0],Halos[i].pos[1],Halos[i].pos[2]);
	}
      
      reallocparts(10);
      for(i=0;i<NumParts;++i)
	{
	  PartsPx[i] = (float) (gsl_rng_uniform(rng)*domainLengths[0] + domainOrigin[0]);
	  PartsPy[i] = (float) (gsl_rng_uniform(rng)*domainLengths[1] + domainOrigin[1]);
	  PartsPz[i] = (float) (gsl_rng_uniform(rng)*domainLengths[2] + domainOrigin[2]);
	  fprintf(stderr,"%d: part ind = %ld, pos = %f|%f|%f\n",ThisTask,i,PartsPx[i],PartsPy[i],PartsPz[i]);
	}
      
      fprintf(stderr,"doing shift to domain\n\n");
      shiftpartshalosglobal2domain();
      
      for(i=0;i<NumHalos;++i)
	fprintf(stderr,"%d: halo ind = %ld, pos = %f|%f|%f\n",ThisTask,i,Halos[i].pos[0],Halos[i].pos[1],Halos[i].pos[2]);
      
      for(i=0;i<NumParts;++i)
	fprintf(stderr,"%d: part ind = %ld, pos = %f|%f|%f\n",ThisTask,i,PartsPx[i],PartsPy[i],PartsPz[i]);
      
      fprintf(stderr,"doing shift to global\n\n");
      shiftpartshalosdomain2global();
      
      for(i=0;i<NumHalos;++i)
	fprintf(stderr,"%d: halo ind = %ld, pos = %f|%f|%f\n",ThisTask,i,Halos[i].pos[0],Halos[i].pos[1],Halos[i].pos[2]);
      
      for(i=0;i<NumParts;++i)
	fprintf(stderr,"%d: part ind = %ld, pos = %f|%f|%f\n",ThisTask,i,PartsPx[i],PartsPy[i],PartsPz[i]);
    }
  
  gsl_rng_free(rng);
  destroydomaindecomp();
  destroyparts();
  destroyhalos();
}

void test_kdtree_nbrsfind(void)
{
  kdTreeData *td;
  float domainLengths[3],centPos[3];
  float searchRadius,dx,dy,dz,rad;
  int NumKeepParts,NumNbrs,periodic,i,j,maxNumNbrs,n,found;
  gsl_rng *rng;
  float *nbrsRad2;
  int *nbrsInd;
  float *px,*py,*pz;
  float L,halfL;
  int k;
  
  L = haloFinderData.BoxLengthSnapshot;
  halfL = (float) (L/2.0);
    
  rng = gsl_rng_alloc(gsl_rng_ranlxd2);
  gsl_rng_set(rng,(unsigned long) (ThisTask+1));
  
  NumKeepParts = 100000;
  domainLengths[0] = L;
  domainLengths[1] = L;
  domainLengths[2] = L;
  maxNumNbrs = 10;
    
  //set max num nbrs
  maxNumNbrs = 1;
  nbrsRad2 = (float*)malloc(sizeof(float)*maxNumNbrs);
  assert(nbrsRad2 != NULL);
  nbrsInd = (int*)malloc(sizeof(int)*maxNumNbrs);
  assert(nbrsInd != NULL);
  
  //make parts
  px = (float*)malloc(sizeof(float)*NumKeepParts);
  assert(px != NULL);
  py = (float*)malloc(sizeof(float)*NumKeepParts);
  assert(py != NULL);
  pz = (float*)malloc(sizeof(float)*NumKeepParts);
  assert(pz != NULL);
  for(i=0;i<NumKeepParts;++i)
    {
      px[i] = (float) (gsl_rng_uniform(rng)*domainLengths[0]);
      py[i] = (float) (gsl_rng_uniform(rng)*domainLengths[1]);
      pz[i] = (float) (gsl_rng_uniform(rng)*domainLengths[2]);
    }
  
  for(periodic=0;periodic<2;++periodic)
    {
      td = buildkdTree(px,py,pz,(int) NumKeepParts,NULL);
      
      for(k=-6;k<=6;++k)
        {
	  if(periodic && (k < 0 || k > 4))
	    continue;
	     
          centPos[0] = (float) (k*halfL/2.0);
          centPos[1] = (float) (k*halfL/2.0);
          centPos[2] = (float) (k*halfL/2.0);
	  
	  searchRadius = (float) (halfL/2.0);
	  NumNbrs = get_nnbrs_kdtree(centPos,searchRadius,periodic,haloFinderData.BoxLengthSnapshot,
				     &nbrsRad2,&nbrsInd,&maxNumNbrs,px,py,pz,td);
	  
	  fprintf(stderr,"%d: NumNbrs = %d\n",ThisTask,NumNbrs);
	  
	  for(i=0;i<NumNbrs;++i)
	    {
	      dx = (float) (fabs(px[nbrsInd[i]] - centPos[0]));
	      dy = (float) (fabs(py[nbrsInd[i]] - centPos[1]));
	      dz = (float) (fabs(pz[nbrsInd[i]] - centPos[2]));
	      if(periodic)
		{
		  if(dx > halfL)
		    dx = L - dx;
		  
		  if(dy > halfL)
		    dy = L - dy;
		  
		  if(dz > halfL)
		    dz = L - dz;
		}
	      
	      rad = (float) (sqrt(dx*dx + dy*dy + dz*dz));
	      if(rad > searchRadius || fabs(rad - sqrt(nbrsRad2[i])) >= 1e-6)
		{
		  fprintf(stderr,"%d: search did not work! rad = %f, searchRadius = %f, rad calc = %f\n",ThisTask,rad,searchRadius,sqrt(nbrsRad2[i]));
		  MPI_Abort(MPI_COMM_WORLD,123);
		}
	    }
	  
	  fprintf(stderr,"%d: all nbrs passed test!\n",ThisTask);
	  
	  searchRadius = searchRadius*searchRadius;
	  j = 0;
	  for(i=0;i<NumKeepParts;++i)
	    {
	      dx = (float) (fabs(px[i] - centPos[0]));
	      dy = (float) (fabs(py[i] - centPos[1]));
	      dz = (float) (fabs(pz[i] - centPos[2]));
	      if(periodic)
		{
		  if(dx > halfL)
		    dx = L - dx;
		  
		  if(dy > halfL)
		    dy = L - dy;
		  
		  if(dz > halfL)
		    dz = L - dz;
		}
	      
	      rad = dx*dx + dy*dy + dz*dz;
	      if(rad <= searchRadius)
		{
		  ++j;
		  found = 0;
		  for(n=0;n<NumNbrs;++n)
		    if(nbrsInd[n] == i)
		      {
			found = 1;
			break;
		      }
		  if(!found)
		    fprintf(stderr,"%d: ind = %d, rad = %le, searchRadius = %le, pos = %f|%f|%f\n",ThisTask,i,rad,searchRadius,
			    px[i],
			    py[i],
			    pz[i]
			    );
		}
	    }
	  
	  if(j != NumNbrs)
	    {
	      fprintf(stderr,"%d: search did not work! NumNbrs = %d, NumNbrs found by meshlink = %d\n",ThisTask,j,NumNbrs);
	      MPI_Abort(MPI_COMM_WORLD,123);
	    }
	  
	  fprintf(stderr,"%d: number of nbrs passed test!\n",ThisTask);
	}
      
      destroykdTree(td);
    }
  
  free(nbrsRad2);
  free(nbrsInd);
  free(px);
  free(py);
  free(pz);
  gsl_rng_free(rng);
}

void test_scattergatherpeakshalos(void)
{
  long i;
  gsl_rng *rng;
  Halo *baseHalos;
  float dx,dy,dz,domainCenter[3];
  
  rng = gsl_rng_alloc(gsl_rng_ranlxd2);
  gsl_rng_set(rng,(unsigned long) (ThisTask+1));
  
  //setup domain decomp
  setupdomaindecomp();
  
  //get domain center
  dx = (float) (haloFinderData.BoxLengthSnapshot/NumDomains[0]);
  dy = (float) (haloFinderData.BoxLengthSnapshot/NumDomains[1]);
  dz = (float) (haloFinderData.BoxLengthSnapshot/NumDomains[2]);
  domainCenter[0] = (float) (domainCoords[0]*dx + dx/2.0);
  domainCenter[1] = (float) (domainCoords[1]*dy + dy/2.0);
  domainCenter[2] = (float) (domainCoords[2]*dz + dz/2.0);
    
  //init halos
  NumHalos = 10;
  Halos = (Halo*)malloc(sizeof(Halo)*NumHalos);
  assert(Halos != NULL);
  baseHalos = (Halo*)malloc(sizeof(Halo)*NumHalos);
  assert(baseHalos != NULL);
  
  for(i=0;i<NumHalos;++i)
    {
      Halos[i].index = i + NumHalos*ThisTask;
      Halos[i].mass = (float) (i + NumHalos*ThisTask);
      Halos[i].pos[0] = (float) (domainCenter[0] + 2.0*(gsl_rng_uniform(rng)-0.5)*(haloFinderData.BoxLengthSnapshot/NumDomains[0]/2.0*0.9));
      Halos[i].pos[1] = (float) (domainCenter[1] + 2.0*(gsl_rng_uniform(rng)-0.5)*(haloFinderData.BoxLengthSnapshot/NumDomains[1]/2.0*0.9));
      Halos[i].pos[2] = (float) (domainCenter[2] + 2.0*(gsl_rng_uniform(rng)-0.5)*(haloFinderData.BoxLengthSnapshot/NumDomains[2]/2.0*0.9));
      baseHalos[i] = Halos[i];
    }
  
  gatherpeakshalos();
  
  scatterpeakshalos();
  
  gsl_heapsort(Halos,(size_t) NumHalos,sizeof(Halo),compHalosMass);
  for(i=0;i<NumHalos;++i)
    {
      //fprintf(stderr,"%d: base mass = %f, mass = %f\n",ThisTask,baseHalos[i].mass,Halos[baseNumHalosPerTask - i - 1].mass);
      //fprintf(stderr,"%d: index = %ld, pos = %lf|%lf|%lf\n",ThisTask,Halos[NumHalos-1-i].index,Halos[NumHalos-1-i].pos[0],Halos[NumHalos-1-i].pos[1],Halos[NumHalos-1-i].pos[2]);
      assert(Halos[NumHalos-1-i].index == baseHalos[i].index);
    }
  
  fprintf(stderr,"%d: passed scatter gather test!\n",ThisTask);
  
  free(baseHalos);
  destroyhalos();
  destroydomaindecomp();
  gsl_rng_free(rng);
}
